#include "MIKE_ZHOU.h"
#include <stdio.h>
#include <string.h>

void MZ_Swap16(void * p)
{
    uint16_t *ptr = p;
    uint16_t x = *ptr;
    x = (x << 8) | (x >> 8);

    *ptr = x;
}

void MZ_Swap32(void * p)
{
    uint32_t *ptr = p;
    uint32_t x = *ptr;
    x = (x << 16) | (x >> 16);
    x = ((x & 0x00FF00FF) << 8) | ((x >> 8) & 0x00FF00FF);

    *ptr = x;
}

void MZ_Swap64(void * p)
{
    uint64_t *ptr = p;
    uint64_t x = *ptr;
    x = (x << 32) | (x >> 32);
    x = ((x & 0x0000FFFF0000FFFF) << 16) | ((x >> 16) & 0x0000FFFF0000FFFF);
    x = ((x & 0x00FF00FF00FF00FF) << 8) | ((x >> 8) & 0x00FF00FF00FF00FF);

    *ptr = x;
}

//传入的字符串和数字必须提前声明 且字符串大小至少为str_len 数组大小至少为str_len%4*3 str_len必须为4的倍数
uint8_t MZ_TransASCII2Pack(uint8_t * str, uint8_t * buf, const uint8_t strLen)
{
    if(strLen % 4)
    {
        return 0;
    }

    uint8_t i = 0;
    memset(buf, 0, strLen / 4 * 3);

    for(i = 0; i < strLen; i++)
    {
        if(str[i] == 0x00)
        {
            str[i] = 0x20;
        }
    }

    for(i = 0; i < strLen / 4; i++)
    {
        buf[3 * i] = (str[4 * i] << 2)|((str[4*i+1]>>4) & 0x03);
        buf[3 * i + 1] = (str[4 * i + 1] << 4)|((str[4*i+2]>>2) & 0x0F);
        buf[3 * i + 2] = (str[4 * i + 2] << 6) | (str[4 * i + 3] & 0x3F);
    }

    return 1;
}

//传入的字符串和数字必须提前声明 且字符串大小至少为str_len 数组大小至少为str_len%4*3 str_len必须为4的倍数
uint8_t MZ_TransPack2ASCII(uint8_t * str, uint8_t * buf, const uint8_t strLen)
{
    if(strLen % 4)
    {
        return 0;
    }

    uint8_t i = 0;

    memset(str, 0, strLen);

    for(i = 0; i < strLen / 4; i++)
    {
        str[4 * i] = (buf[3 * i] >> 2) & 0x3F;
        str[4 * i + 1] = ((buf[3 * i] << 4)&0x30)|(buf[3*i+1]>>4);
        str[4 * i + 2] = ((buf[3 * i + 1] << 2)&0x3C)|(buf[3*i+2]>>6);
        str[4 * i + 3] = buf[3 * i + 2] & 0x3F;
    }

    return 1;
}

void MZ_StatResultUpdate(MZ_StatResultType* res, float newVal)
{
    if(res->num++ == 0)
    {
        res->min = res->max = newVal;
        res->sum = newVal;
        res->avg = newVal;
        res->delta = 0;
        res->m2 = 0;
        res->sampleVar = 0;  // 初始化为0
        res->populationVar = 0;
        return;
    }

    // 极值比较
    res->min = MZ_FMIN(res->min, newVal);
    res->max = MZ_FMAX(res->max, newVal);

    // 增量更新（Welford算法）
    const float delta = newVal - res->avg;
    res->sum += newVal;
    const float newAvg = res->avg + delta / res->num;
    const float delta2 = newVal - newAvg;
    res->m2 += delta * delta2;
    res->avg = newAvg;

    // 计算衍生统计量
    res->delta = res->max - res->min;
    const uint32_t n = res->num;

    if(n > 1)
    {
        res->populationVar = res->m2 / n;
        res->sampleVar = res->m2 / (n - 1);
    }
    else
    {
        res->populationVar = res->sampleVar = 0;
    }
}

MZ_KalmanFilterNormalStruct MZ_KalmanFilterNormalUpdate(MZ_KalmanFilterNormalStruct Stu)
{
   float resultMid = Stu.resultLast;
   float predictionMid = Stu.redictionLast + Stu.Q;
   float kg = predictionMid/(predictionMid + Stu.R);
   Stu.resultNow = resultMid + kg*(Stu.measureNow - resultMid);
   float predictionNow = (1-kg)*predictionMid;
   Stu.redictionLast = predictionNow;
   Stu.resultLast = Stu.resultNow;

   return Stu;
}
